//
// File: detrendIBI_priors.cpp
//
// MATLAB Coder version            : 5.4
// C/C++ source code generated on  : 27-Mar-2025 11:51:36
//

// Include Files
#include "detrendIBI_priors.h"
#include "cat.h"
#include "detrendIBI_priors_data.h"
#include "detrendIBI_priors_initialize.h"
#include "interp1.h"
#include "rt_nonfinite.h"
#include "sparse.h"
#include "spdiags.h"
#include "speye.h"
#include "coder_array.h"
#include <cmath>

// Function Declarations
static void minus(coder::array<double, 1U> &in1,
                  const coder::array<double, 1U> &in2);

// Function Definitions
//
// Arguments    : coder::array<double, 1U> &in1
//                const coder::array<double, 1U> &in2
// Return Type  : void
//
static void minus(coder::array<double, 1U> &in1,
                  const coder::array<double, 1U> &in2)
{
  coder::array<double, 1U> b_in1;
  int i;
  int loop_ub;
  int stride_0_0;
  int stride_1_0;
  if (in2.size(0) == 1) {
    i = in1.size(0);
  } else {
    i = in2.size(0);
  }
  b_in1.set_size(i);
  stride_0_0 = (in1.size(0) != 1);
  stride_1_0 = (in2.size(0) != 1);
  if (in2.size(0) == 1) {
    loop_ub = in1.size(0);
  } else {
    loop_ub = in2.size(0);
  }
  for (i = 0; i < loop_ub; i++) {
    b_in1[i] = in1[i * stride_0_0] - in2[i * stride_1_0];
  }
  in1.set_size(b_in1.size(0));
  loop_ub = b_in1.size(0);
  for (i = 0; i < loop_ub; i++) {
    in1[i] = b_in1[i];
  }
}

//
// Arguments    : const coder::array<double, 2U> &ibi
//                double resampleRate
//                double lambda
//                boolean_T meanCorrection
//                coder::array<double, 2U> &dibi
//                coder::array<double, 2U> &nibi
//                coder::array<double, 2U> &trend
// Return Type  : void
//
void detrendIBI_priors(const coder::array<double, 2U> &ibi, double resampleRate,
                       double lambda, boolean_T meanCorrection,
                       coder::array<double, 2U> &dibi,
                       coder::array<double, 2U> &nibi,
                       coder::array<double, 2U> &trend)
{
  static const signed char c_b[3]{1, -2, 1};
  coder::sparse D2;
  coder::sparse b_I;
  coder::sparse b_b;
  coder::sparse r1;
  coder::array<double, 2U> Vq;
  coder::array<double, 2U> r;
  coder::array<double, 2U> t2;
  coder::array<double, 1U> y;
  coder::array<double, 1U> y2;
  coder::array<double, 1U> z_stat;
  double bsum;
  double cdiff;
  double d;
  double meanIbi;
  int exitg1;
  int firstBlockLength;
  int hi;
  int i;
  int k;
  int lastBlockLength;
  int nblocks;
  boolean_T b;
  if (!isInitialized_detrendIBI_priors) {
    detrendIBI_priors_initialize();
  }
  //  preallocate memory and create default trend of 0.
  hi = ibi.size(0);
  firstBlockLength = ibi.size(0);
  nibi.set_size(ibi.size(0), 2);
  for (i = 0; i < hi; i++) {
    nibi[i] = ibi[i];
  }
  for (i = 0; i < firstBlockLength; i++) {
    nibi[i + nibi.size(0)] = ibi[i + ibi.size(0)];
  }
  trend.set_size(ibi.size(0), 2);
  hi = ibi.size(0) << 1;
  for (i = 0; i < hi; i++) {
    trend[i] = 0.0;
  }
  hi = ibi.size(0);
  for (i = 0; i < hi; i++) {
    trend[i] = ibi[i];
  }
  // time values
  if (ibi.size(0) == 0) {
    cdiff = 0.0;
  } else {
    if (ibi.size(0) <= 1024) {
      firstBlockLength = ibi.size(0);
      lastBlockLength = 0;
      nblocks = 1;
    } else {
      firstBlockLength = 1024;
      nblocks = ibi.size(0) / 1024;
      lastBlockLength = ibi.size(0) - (nblocks << 10);
      if (lastBlockLength > 0) {
        nblocks++;
      } else {
        lastBlockLength = 1024;
      }
    }
    cdiff = ibi[ibi.size(0)];
    for (k = 2; k <= firstBlockLength; k++) {
      cdiff += ibi[(k + ibi.size(0)) - 1];
    }
    for (int ib{2}; ib <= nblocks; ib++) {
      firstBlockLength = (ib - 1) << 10;
      bsum = ibi[firstBlockLength + ibi.size(0)];
      if (ib == nblocks) {
        hi = lastBlockLength;
      } else {
        hi = 1024;
      }
      for (k = 2; k <= hi; k++) {
        bsum += ibi[((firstBlockLength + k) + ibi.size(0)) - 1];
      }
      cdiff += bsum;
    }
  }
  meanIbi = cdiff / static_cast<double>(ibi.size(0));
  // WARNING: cutoff freq depends on lambda and resample rate
  // resample rate
  d = 1.0 / resampleRate;
  if (std::isnan(ibi[0]) || std::isnan(d) || std::isnan(ibi[ibi.size(0) - 1])) {
    t2.set_size(1, 1);
    t2[0] = rtNaN;
  } else if ((d == 0.0) || ((ibi[0] < ibi[ibi.size(0) - 1]) && (d < 0.0)) ||
             ((ibi[ibi.size(0) - 1] < ibi[0]) && (d > 0.0))) {
    t2.set_size(1, 0);
  } else if ((std::isinf(ibi[0]) || std::isinf(ibi[ibi.size(0) - 1])) &&
             (std::isinf(d) || (ibi[0] == ibi[ibi.size(0) - 1]))) {
    t2.set_size(1, 1);
    t2[0] = rtNaN;
  } else if (std::isinf(d)) {
    t2.set_size(1, 1);
    t2[0] = ibi[0];
  } else if ((std::floor(ibi[0]) == ibi[0]) && (std::floor(d) == d)) {
    t2.set_size(1, static_cast<int>((ibi[ibi.size(0) - 1] - ibi[0]) / d) + 1);
    hi = static_cast<int>((ibi[ibi.size(0) - 1] - ibi[0]) / d);
    for (i = 0; i <= hi; i++) {
      t2[i] = ibi[0] + d * static_cast<double>(i);
    }
  } else {
    double apnd;
    bsum = std::floor((ibi[ibi.size(0) - 1] - ibi[0]) / d + 0.5);
    apnd = ibi[0] + bsum * d;
    if (d > 0.0) {
      cdiff = apnd - ibi[ibi.size(0) - 1];
    } else {
      cdiff = ibi[ibi.size(0) - 1] - apnd;
    }
    if (std::abs(cdiff) <
        4.4408920985006262E-16 *
            std::fmax(std::abs(ibi[0]), std::abs(ibi[ibi.size(0) - 1]))) {
      bsum++;
      apnd = ibi[ibi.size(0) - 1];
    } else if (cdiff > 0.0) {
      apnd = ibi[0] + (bsum - 1.0) * d;
    } else {
      bsum++;
    }
    if (bsum >= 0.0) {
      hi = static_cast<int>(bsum);
    } else {
      hi = 0;
    }
    t2.set_size(1, hi);
    if (hi > 0) {
      t2[0] = ibi[0];
      if (hi > 1) {
        t2[hi - 1] = apnd;
        firstBlockLength = (hi - 1) / 2;
        for (k = 0; k <= firstBlockLength - 2; k++) {
          bsum = (static_cast<double>(k) + 1.0) * d;
          t2[k + 1] = ibi[0] + bsum;
          t2[(hi - k) - 2] = apnd - bsum;
        }
        if (firstBlockLength << 1 == hi - 1) {
          t2[firstBlockLength] = (ibi[0] + apnd) / 2.0;
        } else {
          bsum = static_cast<double>(firstBlockLength) * d;
          t2[firstBlockLength] = ibi[0] + bsum;
          t2[firstBlockLength + 1] = apnd - bsum;
        }
      }
    }
  }
  // time values for interp.
  hi = ibi.size(0);
  z_stat.set_size(ibi.size(0));
  for (i = 0; i < hi; i++) {
    z_stat[i] = ibi[i];
  }
  hi = ibi.size(0);
  y.set_size(ibi.size(0));
  for (i = 0; i < hi; i++) {
    y[i] = ibi[i + ibi.size(0)];
  }
  i = ibi.size(0);
  Vq.set_size(1, t2.size(1));
  hi = t2.size(1);
  for (k = 0; k < hi; k++) {
    Vq[k] = 0.0;
  }
  b = (t2.size(1) == 0);
  if (!b) {
    k = 0;
    do {
      exitg1 = 0;
      if (k <= i - 1) {
        if (std::isnan(ibi[k])) {
          exitg1 = 1;
        } else {
          k++;
        }
      } else {
        if (ibi[1] < ibi[0]) {
          k = i >> 1;
          for (hi = 0; hi < k; hi++) {
            cdiff = z_stat[hi];
            nblocks = (i - hi) - 1;
            z_stat[hi] = z_stat[nblocks];
            z_stat[nblocks] = cdiff;
          }
          if (ibi.size(0) > 1) {
            i = ibi.size(0);
            nblocks = ibi.size(0) >> 1;
            for (k = 0; k < nblocks; k++) {
              bsum = y[k];
              firstBlockLength = (i - k) - 1;
              y[k] = y[firstBlockLength];
              y[firstBlockLength] = bsum;
            }
          }
        }
        coder::interp1SplineOrPCHIP(y, t2, Vq, z_stat);
        exitg1 = 1;
      }
    } while (exitg1 == 0);
  }
  y2.set_size(Vq.size(1));
  hi = Vq.size(1);
  for (i = 0; i < hi; i++) {
    y2[i] = Vq[i];
  }
  // resample
  coder::speye(static_cast<double>(y2.size(0)), &b_I);
  r.set_size(y2.size(0) - 2, 3);
  hi = y2.size(0) - 2;
  for (i = 0; i < 3; i++) {
    for (k = 0; k < hi; k++) {
      r[k + r.size(0) * i] = c_b[i];
    }
  }
  coder::spdiags(r, static_cast<double>(y2.size(0)) - 2.0,
                 static_cast<double>(y2.size(0)), &D2);
  D2.ctranspose(&r1);
  r1.mtimes(&D2, &b_b);
  b_b.times(lambda * lambda, &D2);
  b_I.plus(&D2, &r1);
  r1.inv(&b_b);
  b_I.b_minus(&b_b, &D2);
  z_stat.set_size(D2.m);
  hi = D2.m;
  for (i = 0; i < hi; i++) {
    z_stat[i] = 0.0;
  }
  if ((D2.n != 0) && (D2.m != 0) &&
      (D2.colidx[D2.colidx.size(0) - 1] - 1 != 0)) {
    i = D2.n;
    for (int ib{0}; ib < i; ib++) {
      bsum = y2[ib];
      k = D2.colidx[ib];
      firstBlockLength = D2.colidx[ib + 1];
      hi = firstBlockLength - D2.colidx[ib];
      if (hi >= 4) {
        hi = (firstBlockLength - hi) + ((hi / 4) << 2);
        for (lastBlockLength = k; lastBlockLength <= hi - 1;
             lastBlockLength += 4) {
          nblocks = D2.rowidx[lastBlockLength - 1] - 1;
          z_stat[nblocks] = z_stat[nblocks] + D2.d[lastBlockLength - 1] * bsum;
          z_stat[D2.rowidx[lastBlockLength] - 1] =
              z_stat[D2.rowidx[lastBlockLength] - 1] +
              D2.d[lastBlockLength] * bsum;
          nblocks = D2.rowidx[lastBlockLength + 1] - 1;
          z_stat[nblocks] = z_stat[nblocks] + D2.d[lastBlockLength + 1] * bsum;
          nblocks = D2.rowidx[lastBlockLength + 2] - 1;
          z_stat[nblocks] = z_stat[nblocks] + D2.d[lastBlockLength + 2] * bsum;
        }
        firstBlockLength = D2.colidx[ib + 1] - 1;
        for (lastBlockLength = hi; lastBlockLength <= firstBlockLength;
             lastBlockLength++) {
          nblocks = D2.rowidx[lastBlockLength - 1] - 1;
          z_stat[nblocks] = z_stat[nblocks] + D2.d[lastBlockLength - 1] * bsum;
        }
      } else {
        firstBlockLength--;
        for (lastBlockLength = k; lastBlockLength <= firstBlockLength;
             lastBlockLength++) {
          nblocks = D2.rowidx[lastBlockLength - 1] - 1;
          z_stat[nblocks] = z_stat[nblocks] + D2.d[lastBlockLength - 1] * bsum;
        }
      }
    }
  }
  // detrened signal
  if (y2.size(0) == z_stat.size(0)) {
    hi = y2.size(0);
    for (i = 0; i < hi; i++) {
      y2[i] = y2[i] - z_stat[i];
    }
  } else {
    minus(y2, z_stat);
  }
  // trend
  // resample trend back to original time points
  nblocks = t2.size(1) - 1;
  y.set_size(ibi.size(0));
  hi = ibi.size(0);
  for (i = 0; i < hi; i++) {
    y[i] = 0.0;
  }
  if (ibi.size(0) != 0) {
    k = 0;
    do {
      exitg1 = 0;
      if (k <= nblocks) {
        if (std::isnan(t2[k])) {
          exitg1 = 1;
        } else {
          k++;
        }
      } else {
        if (t2[1] < t2[0]) {
          i = (nblocks + 1) >> 1;
          for (hi = 0; hi < i; hi++) {
            cdiff = t2[hi];
            firstBlockLength = nblocks - hi;
            t2[hi] = t2[firstBlockLength];
            t2[firstBlockLength] = cdiff;
          }
          if (y2.size(0) > 1) {
            hi = y2.size(0) - 1;
            nblocks = y2.size(0) >> 1;
            for (k = 0; k < nblocks; k++) {
              bsum = y2[k];
              firstBlockLength = hi - k;
              y2[k] = y2[firstBlockLength];
              y2[firstBlockLength] = bsum;
            }
          }
        }
        hi = ibi.size(0);
        z_stat.set_size(ibi.size(0));
        for (i = 0; i < hi; i++) {
          z_stat[i] = ibi[i];
        }
        coder::interp1SplineOrPCHIP(y2, z_stat, y, t2);
        exitg1 = 1;
      }
    } while (exitg1 == 0);
  }
  hi = trend.size(0);
  for (i = 0; i < hi; i++) {
    trend[i + trend.size(0)] = y[i];
  }
  // interpolation
  if (ibi.size(0) == trend.size(0)) {
    hi = ibi.size(0);
    firstBlockLength = ibi.size(0);
    dibi.set_size(ibi.size(0), 2);
    for (i = 0; i < hi; i++) {
      dibi[i] = ibi[i];
    }
    for (i = 0; i < firstBlockLength; i++) {
      dibi[i + dibi.size(0)] = ibi[i + ibi.size(0)] - trend[i + trend.size(0)];
    }
  } else {
    binary_expand_op(dibi, ibi, trend);
  }
  // detrended IBI
  // %% Note: After removing the trend, the mean value of the ibi
  //  series is near zero and some of the artifact detection methods
  //  detect two many outlers due to a mean near zero
  //  Solution: shift detrended ibi back up to it's orignal mean
  //  by adding orignal mean to detrended ibi.
  if (meanCorrection) {
    firstBlockLength = dibi.size(0) - 1;
    z_stat.set_size(dibi.size(0));
    for (i = 0; i <= firstBlockLength; i++) {
      z_stat[i] = dibi[i + dibi.size(0)] + meanIbi;
    }
    hi = z_stat.size(0);
    for (i = 0; i < hi; i++) {
      dibi[i + dibi.size(0)] = z_stat[i];
    }
    //  see note above
  }
}

//
// File trailer for detrendIBI_priors.cpp
//
// [EOF]
//
